copp\copp\copp3/
formulation.rs

1//! Problem data models and builders for TOPP3/COPP3.
2//!
3//! # Notation policy (math + code)
4//! To help users map paper notation to API fields without ambiguity,
5//! this module follows a dual notation style:
6//! - mathematical definition uses KaTeX, e.g. $a_k = \dot{s}_k^2$ and $b_k = \ddot{s}_k$;
7//! - discrete implementation uses code symbols, e.g. `a[k]`, `b[k]`, `s[k]`.
8//!
9//! # Reference
10//! Wang, Y., Hu, C., Li, Y., Yu, J., Yan, J., Liang, Y., & Jin, Z. (2026).
11//! Online time-optimal trajectory planning along parametric toolpaths with strict constraint
12//! satisfaction and certifiable feasibility guarantee.
13//! *International Journal of Machine Tools and Manufacture*, 215, 104355.
14//! <https://doi.org/10.1016/j.ijmachtools.2025.104355>
15//!
16//! # Stationary-boundary modeling note
17//! This module exposes `num_stationary_max=(start,end)` on builders as a **user input upper
18//! bound** and derives effective `num_stationary` during `build_with_linearization()`.
19//! For typical users, `num_stationary_max=(1,1)` is recommended.
20//!
21//! For stationary boundaries (`a=b=0`), using zero stationary intervals is allowed, but the
22//! boundary-time model can become ill-conditioned because the regular
23//! $c=\frac{\dddot{s}}{\dot{s}}$-constant formulation degenerates near zero speed.
24//! A short boundary neighborhood modeled by $\dddot{s}$-constant stationary intervals is the
25//! practical remedy.
26
27use crate::copp::CoppObjective;
28use crate::copp::constraints::Constraints;
29use crate::diag::{CoppError, check_boundary_state_copp3_valid, check_s_interval_valid};
30use crate::robot::robot_core::{Robot, RobotBasic, RobotTorque};
31use itertools::izip;
32
33const DEFAULT_A_LINEARIZATION_FLOOR: f64 = 1E-10;
34const DEFAULT_NUM_STATIONARY_MAX: (usize, usize) = (1, 1);
35
36/// Borrowed third-order TOPP/COPP profile parts.
37///
38/// This view keeps APIs lightweight for callers that already own separate
39/// `a`/`b` slices while preserving the same semantic grouping as
40/// [`Topp3Profile`]. The tuple layout is `(a, b, num_stationary)`.
41pub type Topp3ProfileRef<'a> = (&'a [f64], &'a [f64], (usize, usize));
42
43/// Mutable borrowed third-order TOPP/COPP profile parts.
44///
45/// This view is intended for in-place profile post-processing. The tuple
46/// layout is `(a, b, num_stationary)`, where only the `a` and `b` slices are
47/// mutable and the stationary boundary counts remain immutable metadata.
48pub type Topp3ProfileMut<'a> = (&'a mut [f64], &'a mut [f64], (usize, usize));
49
50/// Node-based third-order TOPP/COPP profile.
51///
52/// The three fields are tied together by convention: `a.len() == b.len()`,
53/// and `num_stationary` describes stationary head/tail intervals on the same
54/// path grid used with those node profiles.
55#[derive(Clone, Debug, PartialEq)]
56pub struct Topp3Profile {
57    /// Node samples of $a_k = \dot{s}_k^2$.
58    pub a: Vec<f64>,
59    /// Node samples of $b_k = \ddot{s}_k$.
60    pub b: Vec<f64>,
61    /// Stationary boundary interval counts as `(head, tail)`.
62    ///
63    /// `head` counts stationary intervals at the start of the grid, and
64    /// `tail` counts stationary intervals at the end. These markers are used
65    /// by TOPP3/COPP3 interpolation and timing reconstruction.
66    pub num_stationary: (usize, usize),
67}
68
69impl Topp3Profile {
70    /// Build a third-order profile from owned parts.
71    #[inline]
72    pub fn new(a: Vec<f64>, b: Vec<f64>, num_stationary: (usize, usize)) -> Self {
73        Self {
74            a,
75            b,
76            num_stationary,
77        }
78    }
79
80    /// Borrow the profile as `(a, b, num_stationary)` parts.
81    #[inline]
82    pub fn as_parts(&self) -> Topp3ProfileRef<'_> {
83        (&self.a, &self.b, self.num_stationary)
84    }
85
86    /// Mutably borrow the profile as `(a, b, num_stationary)` parts.
87    #[inline]
88    pub fn as_parts_mut(&mut self) -> Topp3ProfileMut<'_> {
89        (&mut self.a, &mut self.b, self.num_stationary)
90    }
91
92    /// Consume the profile and return `(a, b, num_stationary)`.
93    #[inline]
94    pub fn into_parts(self) -> (Vec<f64>, Vec<f64>, (usize, usize)) {
95        (self.a, self.b, self.num_stationary)
96    }
97}
98
99#[inline(always)]
100fn determine_num_stationary_side(a: f64, b: f64, num_stationary_max: usize) -> usize {
101    if a.abs() < f64::EPSILON && b.abs() < f64::EPSILON {
102        num_stationary_max
103    } else {
104        0
105    }
106}
107
108#[inline(always)]
109fn determine_num_stationary_pair(
110    a_boundary: (f64, f64),
111    b_boundary: (f64, f64),
112    num_stationary_max: (usize, usize),
113) -> (usize, usize) {
114    (
115        determine_num_stationary_side(a_boundary.0, b_boundary.0, num_stationary_max.0),
116        determine_num_stationary_side(a_boundary.1, b_boundary.1, num_stationary_max.1),
117    )
118}
119
120/// Prepared TOPP3 problem view.
121///
122/// # Method identity
123/// This is the **read-only runtime view** consumed by TOPP3/COPP3 solvers after
124/// third-order constraints have been linearized.
125///
126/// # Important invariant
127/// The builder precomputes linearized jerk buffers in [`Constraints`](crate::constraints::Constraints):
128/// - `jerk_a_linear`
129/// - `jerk_max_linear`
130///
131/// and this object subsequently holds only `&Constraints` (non-mutable view).
132///
133/// # Why this works
134/// Original third-order inequality includes a nonlinear denominator term:
135/// $$
136/// \sqrt{a(s)}\left(g\_a(s) a(s) + g\_b(s) b(s) + g\_c(s) c(s) + g\_d(s)\right) \le g\_{\text{max}}(s).
137/// $$
138///
139/// Around reference `a_linearization`, it is approximated into affine form:
140/// $$
141/// \left(g\_a(s) + \frac{g\_{\text{max}}(s)}{2a_{\text{lin}}^{3/2}}\right)a + g\_b(s) b + g\_c(s) c
142/// \le \frac{3g\_{\text{max}}(s)}{2a_{\text{lin}}^{1/2}} - g\_d(s).
143/// $$
144/// where $a_{\text{lin}}$ corresponds to code input `a_linearization[k]`.
145///
146/// The affine coefficients are stored into those two buffers for downstream LP/SOCP/RA use.
147pub struct Topp3Problem<'a> {
148    pub(crate) constraints: &'a Constraints,
149    pub(crate) idx_s_start: usize,
150    pub(crate) a_linearization: &'a [f64],
151    pub(crate) a_boundary: (f64, f64),
152    pub(crate) b_boundary: (f64, f64),
153    /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
154    /// boundary conditions and `num_stationary_max`.
155    pub(crate) num_stationary: (usize, usize),
156}
157
158/// Builder for [`Topp3Problem`](crate::solver::topp3_lp::Topp3Problem), including optional in-build linearization.
159///
160/// # Side effect notice
161/// `build_with_linearization()` updates cached affine linearization data inside [`Constraints`](crate::constraints::Constraints).
162/// Raw jerk constraints remain unchanged.
163pub struct Topp3ProblemBuilder<'a> {
164    /// Mutable constraint storage used to build linearized TOPP3 problem data.
165    pub constraints: &'a mut Constraints,
166    /// Start station index of the optimization interval.
167    pub idx_s_start: usize,
168    /// Reference profile `a[k]` used to linearize third-order constraints.
169    pub a_linearization: &'a [f64],
170    /// Boundary values of `a=(a_start, a_final)`.
171    pub a_boundary: (f64, f64),
172    /// Boundary values of `b=(b_start, b_final)`.
173    pub b_boundary: (f64, f64),
174    /// User-input upper bound of stationary intervals at (start, end).
175    pub num_stationary_max: (usize, usize),
176    /// Denominator floor for stable evaluation of `1/sqrt(a_linearization)` near `a=0`.
177    ///
178    /// Effective usage in linearization is:
179    /// $$
180    /// \frac{1}{\sqrt{\max(a_{lin}, a_{floor})}}.
181    /// $$
182    /// Discrete code form:
183    /// `1.0 / max(a_linearization, a_linearization_floor).sqrt()`.
184    ///
185    /// More details are available in the [`Topp3Problem`](crate::solver::topp3_lp::Topp3Problem) documentation.
186    pub a_linearization_floor: f64,
187}
188
189impl<'a> Topp3ProblemBuilder<'a> {
190    /// Create a TOPP3 builder with required fields.
191    ///
192    /// Defaults:
193    /// - `num_stationary_max = (1, 1)`
194    /// - `a_linearization_floor = 1E-10`
195    pub fn new<M: RobotBasic>(
196        robot: &'a mut Robot<M>,
197        idx_s_start: usize,
198        a_linearization: &'a [f64],
199        a_boundary: (f64, f64),
200        b_boundary: (f64, f64),
201    ) -> Self {
202        Self {
203            constraints: &mut robot.constraints,
204            idx_s_start,
205            a_linearization,
206            a_boundary,
207            b_boundary,
208            num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
209            a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
210        }
211    }
212
213    /// Create a TOPP3 builder with required fields.
214    ///
215    /// Defaults:
216    /// - `num_stationary_max = (1, 1)`
217    /// - `a_linearization_floor = 1E-10`
218    pub fn with_constraint(
219        constraints: &'a mut Constraints,
220        idx_s_start: usize,
221        a_linearization: &'a [f64],
222        a_boundary: (f64, f64),
223        b_boundary: (f64, f64),
224    ) -> Self {
225        Self {
226            constraints,
227            idx_s_start,
228            a_linearization,
229            a_boundary,
230            b_boundary,
231            num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
232            a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
233        }
234    }
235
236    /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
237    ///
238    /// See module-level **Stationary-boundary modeling note** for guidance.
239    #[inline]
240    pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
241        self.num_stationary_max = (num_stationary_max, num_stationary_max);
242        self
243    }
244
245    /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
246    ///
247    /// See module-level **Stationary-boundary modeling note** for guidance.
248    #[inline]
249    pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
250        self.num_stationary_max = num_stationary_max;
251        self
252    }
253
254    /// Set denominator floor used in linearization.
255    #[inline]
256    pub fn with_a_linearization_floor(mut self, floor: f64) -> Self {
257        self.a_linearization_floor = floor;
258        self
259    }
260
261    /// Build a TOPP3 problem and linearize third-order constraints in one step.
262    ///
263    /// This validates boundaries/interval/floor first, then writes linearized jerk buffers.
264    pub fn build_with_linearization(self) -> Result<Topp3Problem<'a>, CoppError> {
265        check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
266        if self.a_linearization.is_empty() {
267            return Err(CoppError::InvalidInput(
268                "Topp3ProblemBuilder::build_with_linearization".into(),
269                "a_linearization cannot be empty".into(),
270            ));
271        }
272        let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
273        check_s_interval_valid(
274            "Topp3ProblemBuilder::build_with_linearization",
275            self.idx_s_start,
276            idx_s_final,
277        )?;
278        if self.a_linearization_floor <= 0.0 {
279            return Err(CoppError::InvalidInput(
280                "Topp3ProblemBuilder::build_with_linearization".into(),
281                format!(
282                    "a_linearization_floor must be positive, got {}",
283                    self.a_linearization_floor
284                ),
285            ));
286        }
287
288        self.constraints
289            .linearize_constraint_3order_with_floor(
290                self.a_linearization,
291                self.idx_s_start,
292                self.a_linearization_floor,
293            )
294            .map_err(|e| {
295                CoppError::InvalidInput(
296                    "Topp3ProblemBuilder::build_with_linearization".into(),
297                    format!("linearize_constraint_3order failed: {e}"),
298                )
299            })?;
300
301        let num_stationary = determine_num_stationary_pair(
302            self.a_boundary,
303            self.b_boundary,
304            self.num_stationary_max,
305        );
306
307        Ok(Topp3Problem {
308            constraints: &*self.constraints,
309            idx_s_start: self.idx_s_start,
310            a_linearization: self.a_linearization,
311            a_boundary: self.a_boundary,
312            b_boundary: self.b_boundary,
313            num_stationary,
314        })
315    }
316}
317
318/// The problem of COPP3.
319/// # Arguments
320/// * `robot` - A robot with torque implemented, which defines the constraints and dynamic of the problem.
321/// * `objectives` - Objectives for COPP3 optimization.
322/// * `idx_s_start` - The starting index along the path (reached).
323/// * `a_linearization` - Linearization reference profile for third-order constraints.
324/// * `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.
325/// * `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.
326/// * `num_stationary=(start,end)` - Effective stationary intervals derived in `build_with_linearization()`.
327pub struct Copp3Problem<'a, M: RobotTorque> {
328    pub(crate) robot: &'a mut Robot<M>,
329    pub(crate) objectives: &'a [CoppObjective<'a>],
330    pub(crate) idx_s_start: usize,
331    pub(crate) a_linearization: &'a [f64],
332    pub(crate) a_boundary: (f64, f64),
333    pub(crate) b_boundary: (f64, f64),
334    /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
335    /// boundary conditions and `num_stationary_max`.
336    pub(crate) num_stationary: (usize, usize),
337}
338
339/// Builder for [`Copp3Problem`](crate::solver::copp3_socp::Copp3Problem).
340pub struct Copp3ProblemBuilder<'a, M: RobotTorque> {
341    /// A robot with torque implemented, which defines the constraints and dynamic of the problem.
342    pub robot: &'a mut Robot<M>,
343    /// Objectives for COPP3 optimization.
344    pub objectives: &'a [CoppObjective<'a>],
345    /// The starting index along the path (reached).
346    pub idx_s_start: usize,
347    /// Linearization reference profile for third-order constraints.
348    pub a_linearization: &'a [f64],
349    /// `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.
350    pub a_boundary: (f64, f64),
351    /// `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.
352    pub b_boundary: (f64, f64),
353    /// User-input upper bound of stationary intervals at (start, end).
354    pub num_stationary_max: (usize, usize),
355    /// Denominator floor for stable evaluation of `1/sqrt(a_linearization)` near `a=0`.
356    ///
357    /// Effective usage in linearization is:
358    /// $$
359    /// \frac{1}{\sqrt{\max(a_{lin}, a_{floor})}}.
360    /// $$
361    /// Discrete code form:
362    /// `1.0 / max(a_linearization, a_linearization_floor).sqrt()`.
363    ///
364    /// More details are available in the [`Topp3Problem`](crate::solver::topp3_lp::Topp3Problem) documentation.
365    pub a_linearization_floor: f64,
366}
367
368impl<'a, M: RobotTorque> Copp3ProblemBuilder<'a, M> {
369    /// Create a COPP3 builder with required fields.
370    ///
371    /// Defaults:
372    /// - `num_stationary_max = (1, 1)`
373    /// - `a_linearization_floor = 1E-10`
374    pub fn new(
375        robot: &'a mut Robot<M>,
376        objectives: &'a [CoppObjective<'a>],
377        idx_s_start: usize,
378        a_linearization: &'a [f64],
379        a_boundary: (f64, f64),
380        b_boundary: (f64, f64),
381    ) -> Self {
382        Self {
383            robot,
384            objectives,
385            idx_s_start,
386            a_linearization,
387            a_boundary,
388            b_boundary,
389            num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
390            a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
391        }
392    }
393
394    /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
395    ///
396    /// See module-level **Stationary-boundary modeling note** for guidance.
397    #[inline]
398    pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
399        self.num_stationary_max = (num_stationary_max, num_stationary_max);
400        self
401    }
402
403    /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
404    ///
405    /// See module-level **Stationary-boundary modeling note** for guidance.
406    #[inline]
407    pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
408        self.num_stationary_max = num_stationary_max;
409        self
410    }
411
412    /// Set denominator floor used in linearization.
413    #[inline]
414    pub fn with_a_linearization_floor(mut self, floor: f64) -> Self {
415        self.a_linearization_floor = floor;
416        self
417    }
418
419    /// Build a validated COPP3 problem and linearize third-order constraints in one step.
420    ///
421    /// This validates boundaries/interval/floor first, then writes linearized jerk buffers.
422    pub fn build_with_linearization(self) -> Result<Copp3Problem<'a, M>, CoppError> {
423        check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
424        if self.a_linearization.is_empty() {
425            return Err(CoppError::InvalidInput(
426                "Copp3ProblemBuilder::build_with_linearization".into(),
427                "a_linearization cannot be empty".into(),
428            ));
429        }
430        let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
431        check_s_interval_valid(
432            "Copp3ProblemBuilder::build_with_linearization",
433            self.idx_s_start,
434            idx_s_final,
435        )?;
436        self.robot
437            .constraints
438            .check_s_in_bounds(self.idx_s_start, self.a_linearization.len())?;
439        if self.a_linearization_floor <= 0.0 {
440            return Err(CoppError::InvalidInput(
441                "Copp3ProblemBuilder::build_with_linearization".into(),
442                format!(
443                    "a_linearization_floor must be positive, got {}",
444                    self.a_linearization_floor
445                ),
446            ));
447        }
448
449        self.robot
450            .constraints
451            .linearize_constraint_3order_with_floor(
452                self.a_linearization,
453                self.idx_s_start,
454                self.a_linearization_floor,
455            )
456            .map_err(|e| {
457                CoppError::InvalidInput(
458                    "Copp3ProblemBuilder::build_with_linearization".into(),
459                    format!("linearize_constraint_3order failed: {e}"),
460                )
461            })?;
462
463        let num_stationary = determine_num_stationary_pair(
464            self.a_boundary,
465            self.b_boundary,
466            self.num_stationary_max,
467        );
468
469        Ok(Copp3Problem {
470            robot: self.robot,
471            objectives: self.objectives,
472            idx_s_start: self.idx_s_start,
473            a_linearization: self.a_linearization,
474            a_boundary: self.a_boundary,
475            b_boundary: self.b_boundary,
476            num_stationary,
477        })
478    }
479}
480
481impl<'a, M: RobotTorque> Copp3Problem<'a, M> {
482    /// Update linearization profile.
483    #[inline]
484    pub fn set_a_linearization(&mut self, a_linearization: &'a [f64]) {
485        self.a_linearization = a_linearization;
486    }
487
488    /// Backward-compatible alias for `set_a_linearization`.
489    #[inline]
490    pub fn set_a_linear(&mut self, a_linearization: &'a [f64]) {
491        self.set_a_linearization(a_linearization);
492    }
493
494    /// Update objective list.
495    #[inline]
496    pub fn set_objective(&mut self, objective: &'a [CoppObjective<'a>]) {
497        self.objectives = objective;
498    }
499
500    /// Convert to the TOPP3 view that shares interval/boundary/linearization fields.
501    pub fn as_topp3_problem(&self) -> Topp3Problem<'_> {
502        Topp3Problem {
503            constraints: &self.robot.constraints,
504            idx_s_start: self.idx_s_start,
505            a_linearization: self.a_linearization,
506            a_boundary: self.a_boundary,
507            b_boundary: self.b_boundary,
508            num_stationary: self.num_stationary,
509        }
510    }
511}
512
513/// Get the weight of `a` for value function.
514/// Time loss = \sum_{k=0}^n weight_a[k] / sqrt(a[k]) \approx Time
515pub(crate) fn get_weight_a_topp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
516    let n = s.len() - 1;
517    let mut weight_a = vec![0.0; s.len()];
518    if num_stationary.0 > 0 {
519        weight_a[num_stationary.0] =
520            0.5 * (5.0 * s[num_stationary.0] + s[num_stationary.0 + 1] - 6.0 * s[0]);
521    }
522    if num_stationary.1 > 0 {
523        weight_a[n - num_stationary.1] =
524            0.5 * (6.0 * s[n] - 5.0 * s[n - num_stationary.1] - s[n - num_stationary.1 - 1]);
525    }
526    weight_a
527        .iter_mut()
528        .skip(1)
529        .zip(s.windows(3))
530        .skip(num_stationary.0)
531        .take(n - num_stationary.0 - num_stationary.1 - 1)
532        .for_each(|(w_a, s_slice)| {
533            *w_a = 0.5 * (s_slice[2] - s_slice[0]);
534        });
535
536    weight_a
537}
538
539/// Get the weight of `a` for value function.
540/// Loss = weight[0] * loss_average_left / sqrt(a[num_stationary.0]) + weight[n] * loss_average_right / sqrt(a[n - num_stationary.1]) + \sum_{k=num_stationary.0}^{n-num_stationary.1-1} weight_a[k] * loss[k] / sqrt(a[k])
541pub(crate) fn get_weight_a_copp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
542    let n = s.len() - 1;
543    let mut weight_a = vec![0.0; s.len()];
544    if num_stationary.0 > 0 {
545        let s_n1 = s[num_stationary.0];
546        weight_a[num_stationary.0] = 0.5 * (s[num_stationary.0 + 1] - s_n1);
547        weight_a[0] = 3.0 * (s_n1 - s[0]);
548    }
549    if num_stationary.1 > 0 {
550        let s_n2 = s[n - num_stationary.1];
551        weight_a[n - num_stationary.1] = 0.5 * (s_n2 - s[n - num_stationary.1 - 1]);
552        weight_a[n] = 3.0 * (s[n] - s_n2);
553    }
554    weight_a
555        .iter_mut()
556        .skip(1)
557        .zip(s.windows(3))
558        .skip(num_stationary.0)
559        .take(n - num_stationary.0 - num_stationary.1 - 1)
560        .for_each(|(w_a, s_slice)| {
561            *w_a = 0.5 * (s_slice[2] - s_slice[0]);
562        });
563
564    weight_a
565}
566
567/// Get the `a` and `b` profiles for stationary intervals.
568pub(crate) fn set_ab_stationary_topp3<const START: bool>(
569    s: &[f64],
570    a: &mut [f64],
571    b: &mut [f64],
572    a_stationary: f64,
573    num_stationary: usize,
574) {
575    if num_stationary == 0 {
576        return;
577    }
578    if START {
579        let s_start = s[0];
580        let ds_start = s[num_stationary] - s_start;
581        *a.first_mut().unwrap() = 0.0;
582        *b.first_mut().unwrap() = 0.0;
583        a[num_stationary] = a_stationary;
584        b[num_stationary] = a_stationary / (1.5 * ds_start);
585        if num_stationary > 1 {
586            for (a_k, b_k, &s_k) in izip!(a.iter_mut(), b.iter_mut(), s.iter())
587                .skip(1)
588                .take(num_stationary - 1)
589            {
590                let dsk_start = s_k - s_start;
591                let mut alpha = dsk_start / ds_start;
592                alpha *= alpha.cbrt();
593                *a_k = a_stationary * alpha;
594                *b_k = *a_k / (1.5 * dsk_start);
595            }
596        }
597    } else {
598        let &s_final = s.last().unwrap();
599        let n = s.len() - 1;
600        let ds_final = s[n - num_stationary] - s_final;
601        *a.last_mut().unwrap() = 0.0;
602        *b.last_mut().unwrap() = 0.0;
603        a[a.len() - 1 - num_stationary] = a_stationary;
604        b[b.len() - 1 - num_stationary] = a_stationary / (1.5 * ds_final);
605        if num_stationary > 1 {
606            for (a_k, b_k, &s_k) in izip!(a.iter_mut().rev(), b.iter_mut().rev(), s.iter().rev())
607                .skip(1)
608                .take(num_stationary - 1)
609            {
610                let dsk_final = s_k - s_final;
611                let mut alpha = dsk_final / ds_final;
612                alpha *= alpha.cbrt();
613                *a_k = a_stationary * alpha;
614                *b_k = *a_k / (1.5 * dsk_final);
615            }
616        }
617    }
618}
619
620#[cfg(test)]
621mod tests {
622    use super::determine_num_stationary_pair;
623
624    #[test]
625    fn test_determine_num_stationary_pair_respects_boundary_state() {
626        let pair = determine_num_stationary_pair((0.0, 0.0), (0.0, 0.0), (2, 3));
627        assert_eq!(pair, (2, 3));
628
629        let pair = determine_num_stationary_pair((1.0, 0.0), (0.0, 0.0), (2, 3));
630        assert_eq!(pair, (0, 3));
631
632        let pair = determine_num_stationary_pair((0.0, 1.0), (0.0, 1.0), (2, 3));
633        assert_eq!(pair, (2, 0));
634    }
635}
copp\copp\copp3/formulation.rs

copp\copp\copp3/
formulation.rs
